import re
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.widgets import CheckButtons

# ==================== 配置文件路径 ====================
log_path = r"C:\Users\LSQ_CX\Desktop\下午日志详细分析\control_node_3168_63242.log"

# 存储数据
estimate_points = []  # 目标估计位置
mydrone_points = []   # 自身无人机位置

# 正则表达式
pattern_estimate = re.compile(r"estimate_info:\s*\[([-\d.eE+]+),\s*([-\d.eE+]+),\s*([-\d.eE+]+)")
pattern_mydrone = re.compile(r"mydrone_enu:\s*\[\s*([-\d.eE+-]+)\s+([-\d.eE+-]+)\s+([-\d.eE+-]+)")

# ==================== 解析日志 ====================
with open(log_path, 'r', encoding='utf-8') as f:
    for line in f:
        # 提取 estimate_info 前三个值
        match_est = pattern_estimate.search(line)
        if match_est:
            try:
                x = float(match_est.group(1))
                y = float(match_est.group(2))
                z = float(match_est.group(3))
                estimate_points.append((x, y, z))
            except ValueError:
                continue

        # 提取 mydrone_enu
        match_mine = pattern_mydrone.search(line)
        if match_mine:
            try:
                x = float(match_mine.group(1))
                y = float(match_mine.group(2))
                z = float(match_mine.group(3))
                mydrone_points.append((x, y, z))
            except ValueError:
                continue

# 转为 numpy 数组
estimate_points = np.array(estimate_points)
mydrone_points = np.array(mydrone_points)

print(f"提取到 estimate_info 点数: {len(estimate_points)}")
print(f"提取到 mydrone_enu 点数: {len(mydrone_points)}")

# ==================== 初始化绘图 ====================
fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')

# 绘制轨迹（初始为空）
line_est, = ax.plot([], [], [], color='blue', linewidth=2, marker='o', markersize=2, label='Estimated Target')
line_mine, = ax.plot([], [], [], color='red', linewidth=2, marker='^', markersize=2, label='Own Drone')

# 设置初始数据
if len(estimate_points) > 0:
    line_est.set_data(estimate_points[:, 0], estimate_points[:, 1])
    line_est.set_3d_properties(estimate_points[:, 2])

if len(mydrone_points) > 0:
    line_mine.set_data(mydrone_points[:, 0], mydrone_points[:, 1])
    line_mine.set_3d_properties(mydrone_points[:, 2])

# ========== 自定义函数：根据可见轨迹更新坐标轴范围 ==========
def update_axes():
    ax.clear()

    # 收集当前要绘制的数据
    all_x, all_y, all_z = [], [], []

    if line_est.get_visible() and len(estimate_points) > 0:
        all_x.extend(estimate_points[:, 0])
        all_y.extend(estimate_points[:, 1])
        all_z.extend(estimate_points[:, 2])
        ax.plot(estimate_points[:, 0], estimate_points[:, 1], estimate_points[:, 2],
                color='blue', linewidth=2, marker='o', markersize=2, label='Estimated Target')

    if line_mine.get_visible() and len(mydrone_points) > 0:
        all_x.extend(mydrone_points[:, 0])
        all_y.extend(mydrone_points[:, 1])
        all_z.extend(mydrone_points[:, 2])
        ax.plot(mydrone_points[:, 0], mydrone_points[:, 1], mydrone_points[:, 2],
                color='red', linewidth=2, marker='^', markersize=2, label='Own Drone')

    # 如果有数据，则设置坐标轴范围
    if all_x:
        min_x, max_x = min(all_x), max(all_x)
        min_y, max_y = min(all_y), max(all_y)
        min_z, max_z = min(all_z), max(all_z)

        # 添加一点边距
        padding_x = (max_x - min_x) * 0.05
        padding_y = (max_y - min_y) * 0.05
        padding_z = (max_z - min_z) * 0.05

        ax.set_xlim(min_x - padding_x, max_x + padding_x)
        ax.set_ylim(min_y - padding_y, max_y + padding_y)
        ax.set_zlim(min_z - padding_z, max_z + padding_z)
    else:
        ax.set_xlim(0, 1)
        ax.set_ylim(0, 1)
        ax.set_zlim(0, 1)

    ax.set_xlabel("Easting (X)")
    ax.set_ylabel("Northing (Y)")
    ax.set_zlabel("Height (Z)")
    ax.set_title("3D Trajectory: Estimated Target vs Own Drone")
    ax.legend()
    fig.canvas.draw_idle()  # 重绘画布

# ========== 初始化显示状态 ==========
line_est.set_visible(True)
line_mine.set_visible(True)

# 第一次绘制前先更新坐标轴
update_axes()

# ========== 添加勾选控件 ==========
ax_check = plt.axes([0.1, 0.05, 0.2, 0.1])
labels = ['Show Estimate', 'Show My Drone']
check_btns = CheckButtons(ax_check, labels, [True, True])

# ========== 勾选事件响应 ==========
def on_check_clicked(label):
    if label == 'Show Estimate':
        line_est.set_visible(not line_est.get_visible())
    elif label == 'Show My Drone':
        line_mine.set_visible(not line_mine.get_visible())
    # 每次切换后重新计算坐标轴范围
    update_axes()

check_btns.on_clicked(on_check_clicked)

# ========== 显示 ==========
plt.show()